#include "Histogram.h"

Histogram::Histogram (std::vector<FeatureDescriptor *> * features,std::vector<int> binsizes,std::vector<float> max_val,std::vector<float> min_val)
{
	nr_dim = 0;
	if(!features->empty()){setup(features,binsizes,max_val,min_val);}
}

void Histogram::setup(std::vector<FeatureDescriptor *> * features, std::vector<int> binsizes, std::vector<float> max_val, std::vector<float> min_val)
{
	int multi	= 1;
	nr_dim		= binsizes.size();
	bin_size	= new float[nr_dim];
	min			= new float[nr_dim];
	steps 		= new int[nr_dim];
	nr_bins 	= new int[nr_dim];
	tmp_bin	 	= new int[nr_dim];
	for(int i = 0; i < nr_dim;i++)
	{
		steps[i] 		= multi;
		min[i] 			= min_val.at(i);
		nr_bins[i]		= binsizes.at(i) - 1;
		bin_size[i]		= 0.9999f*((float)binsizes.at(i))/(max_val.at(i)-min[i]);
		multi 			*= binsizes.at(i);
		//ROS_INFO("steps[%i] = %i",i,steps[i]);
	}
	
	descriptor 			= new float[multi];
	descriptor_length 	= multi;
	for(int i = 0; i < multi; i++)
	{
		descriptor[i] = 0;
	}
	type = 3;
	
	for(unsigned int i = 0; i < features->size();i++)
	{
		FeatureDescriptor * feat = features->at(i);
		insert(feat);
	}
	//Normalize histogram
	normalize();
	//exit(0);
}


void Histogram::insert(FeatureDescriptor * feature)
{
	//printf("feature_desc: ");
	float * feature_desc = feature->descriptor;
	int a;
	unsigned int binid = 0;
	for(int i = 0; i < nr_dim;i++)
	{
		a = (feature_desc[i] - min[i])*bin_size[i];
		//printf("[%f->%i]",feature_desc[i],a);
		if(a < 0)					{a = 0;}
		else if(a > (int)(nr_bins[i]))		{a = nr_bins[i];}
		binid+=a*steps[i];
	}
	//printf("\n");
	descriptor[binid]++;
}

inline void Histogram::eccAddFinal(int * offset, int dim)
{
	bool ok = true;
	for(int j = 0; j < nr_dim; j++)
	{
		if(offset[j]+tmp_bin[j] < 0 || offset[j]+tmp_bin[j] >= nr_bins[j]){ok = false;break;} 
	}
	if(ok)
	{
		for(int j = 0; j < nr_dim; j++){tmp_bin[j]+=offset[j];}
		descriptor[bin_index(tmp_bin)]++;
		for(int j = 0; j < nr_dim; j++){tmp_bin[j]-=offset[j];}
	}

	for(int i = dim; i < nr_dim; i++)
	{
		if(offset[i] != 0)
		{
			offset[i] = -offset[i];
			eccAddFinal(offset,i+1);
			offset[i] = -offset[i];
		}
	}
}

inline void Histogram::eccAdd(int * offset,int ecc, int dim){
	eccAddFinal(offset, 0);
	
	if(ecc > 0)
	{
		ecc--;
		for(int i = dim; i < nr_dim; i++)
		{
			offset[i]++;
			eccAdd(offset,ecc,i);
			offset[i]--;
		}
	}
}

inline int Histogram::bin_index(int * bin){
	int ret = 0;
	for(int i = 0; i < nr_dim;i++)
	{
		ret+=bin[i]*steps[i];
	}
	return ret;
}


Histogram::~Histogram()
{
	//ROS_INFO("deleted histogram");
	delete[] nr_bins;
	delete[] tmp_bin;
	delete[] bin_size;
	delete[] min;
	delete[] steps;
	
	nr_dim = 0;
	nr_bins = 0;
	tmp_bin = 0;
	bin_size = 0;
	min = 0;
	steps = 0;
	descriptor = 0;
	descriptor_length = 0;
}
